/* Copyright 2017 The Apollo Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "modules/map/hdmap/adapter/odr_parser/lanes_odr_parser.h"

#include <string>
#include <algorithm>
#include <string>
#include <vector>
#include <limits>

#include "modules/common/configs/config_gflags.h"
#include "modules/map/hdmap/adapter/xml_parser/util_xml_parser.h"
#include "modules/map/hdmap/adapter/odr_parser/util_odr_parser.h"

namespace {
double ToMPS(double speed) { return speed * 1000.0 / 3600.0; }
bool IsReferenceLane(int lane_id) { return lane_id == 0; }
bool ToBool(const std::string& s) {
  std::string upper_str = apollo::hdmap::adapter::UtilXmlParser::ToUpper(s);
  return (upper_str != "FALSE" && upper_str != "0");
}
};

namespace apollo {
namespace hdmap {
namespace adapter {

Status LanesOdrParser::Parse(std::string road_id, const tinyxml2::XMLElement& xml_node,
                             std::vector<LaneSection*>* sections) {
  CHECK_NOTNULL(sections);
  
  const auto lanes_node = xml_node.FirstChildElement("lanes");
  CHECK_NOTNULL(lanes_node);
  const tinyxml2::XMLElement* sub_node =
      lanes_node->FirstChildElement("laneSection");
  CHECK_NOTNULL(sub_node);

  size_t section_cnt = 0;
  while (sub_node) {
    LaneSection* section = new LaneSection();
    RETURN_IF_ERROR(ParseLaneSection(road_id, section_cnt, *sub_node, section));

    section->e = -1.0;
    if (!sections->empty()) {
      sections->back()->e = section->s;
    }
    sections->push_back(section);

    sub_node = sub_node->NextSiblingElement("laneSection");
    section_cnt++;
  }

  // set end
  // we assume the sections are sorted by increasing the start offset
  for (size_t i = 0; i < sections->size(); i++) {
    LaneSection* section = sections->at(i);
    section->e = std::numeric_limits<float>::max();
    bool found = false;
    for (size_t j = i+1; j < sections->size(); j++) {
      LaneSection* section2 = sections->at(j);
      if (section2->s - 0.0000001 > section->s && section2->s < section->e) {
        section->e = section2->s;
        found = true;
      }
    }
    if (!found) {
      section->e = -1;
    }
  }

  CHECK_GT(sections->size(), 0);

  return Status::OK();
}

Status LanesOdrParser::ParseLaneSection(std::string road_id, size_t sectionIndex, const tinyxml2::XMLElement& xml_node,
                                        LaneSection* lanes) {
  CHECK_NOTNULL(lanes);

  int checker = xml_node.QueryDoubleAttribute("s", &lanes->s);
  if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lanesection s attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  // left
  const tinyxml2::XMLElement* sub_node = xml_node.FirstChildElement("left");
  if (sub_node) {
    sub_node = sub_node->FirstChildElement("lane");
    while (sub_node) {
      Lane* lane = new Lane();
      std::string prefix = road_id + "_"+ std::to_string(sectionIndex)+"_";
      RETURN_IF_ERROR(ParseLane(prefix, *sub_node, lane));

      lanes->left.push_back(lane);
      sub_node = sub_node->NextSiblingElement("lane");
    }
  }

  // center
  lanes->center = NULL;
  sub_node = xml_node.FirstChildElement("center");
  //CHECK_NOTNULL(sub_node);
  if (sub_node) {
    Lane* reference_lane = new Lane();
    std::string prefix = road_id + "_"+ std::to_string(sectionIndex)+"_";
    sub_node = sub_node->FirstChildElement("lane");
    CHECK_NOTNULL(sub_node);
    RETURN_IF_ERROR(ParseLane(prefix, *sub_node, reference_lane));

    lanes->center = reference_lane;
  }  
  //else {
  //  RETURN_IF_ERROR(SetNullCenterLane(reference_lane));
  //}

  // right
  sub_node = xml_node.FirstChildElement("right");
  if (sub_node) {
    sub_node = sub_node->FirstChildElement("lane");
    while (sub_node) {
      Lane* lane = new Lane();
      std::string prefix = road_id + "_"+ std::to_string(sectionIndex)+"_";
      RETURN_IF_ERROR(ParseLane(prefix, *sub_node, lane));

      lanes->right.push_back(lane);
      sub_node = sub_node->NextSiblingElement("lane");
    }
  }
  return Status::OK();
}

Status LanesOdrParser::ParseLane(std::string uidPrefix, const tinyxml2::XMLElement& xml_node,
                                 Lane* lane) {
  CHECK_NOTNULL(lane);

  // lane id
  std::string id;
  int checker = UtilXmlParser::QueryStringAttribute(xml_node, "id", &id);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse lane id.";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }
  lane->id = id;

  lane->uid= uidPrefix+id;
  

  // lane type
  std::string lane_type;
  checker = UtilXmlParser::QueryStringAttribute(xml_node, "type", &lane_type);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse lane type.";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }
  std::string level;
  checker = UtilXmlParser::QueryStringAttribute(xml_node, "level", &level);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse lane level.";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }
  lane->level = ToBool(level);

  LaneType pb_lane_type;
  Status success = ToLaneType(lane_type, &pb_lane_type);
  if (!success.ok()) {
    std::string err_msg = "Error convert lane type to pb lane type.";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }
  lane->type = pb_lane_type;

  // reference line
  //if (IsReferenceLane(id)) {
  //  return Status::OK();
  //}

  // link
  const tinyxml2::XMLElement* link_node = xml_node.FirstChildElement("link");
  if (link_node) {
    RETURN_IF_ERROR(ParseLaneLink(*link_node, lane));
  }

  // width
  RETURN_IF_ERROR(ParseWidth(xml_node, lane));

  // if no width element, look up border element
  if (lane->widths.empty()) {
    RETURN_IF_ERROR(ParseBorder(xml_node, lane));
  }

  // roadMark
  RETURN_IF_ERROR(ParseRoadMark(xml_node, lane));

  // speed
  RETURN_IF_ERROR(ParseSpeed(xml_node, lane));

  return Status::OK();
}

Status LanesOdrParser::ParseWidth(const tinyxml2::XMLElement& xml_node,
                                  Lane* lane) {
  auto sub_node = xml_node.FirstChildElement("width");
  while (sub_node) {
    LaneWidth width;

    RETURN_IF_ERROR(ParseWidthAttributes(sub_node, width));

    lane->widths.push_back(width);
    sub_node = sub_node->NextSiblingElement("width");
  }

  return Status::OK();
}

Status LanesOdrParser::ParseBorder(const tinyxml2::XMLElement& xml_node,
                                  Lane* lane) {
  auto sub_node = xml_node.FirstChildElement("border");
  while (sub_node) {
    LaneWidth width;

    RETURN_IF_ERROR(ParseWidthAttributes(sub_node, width));

    lane->borders.push_back(width);
    sub_node = sub_node->NextSiblingElement("border");
  }

  return Status::OK();
}

Status LanesOdrParser::ParseWidthAttributes(const tinyxml2::XMLElement* sub_node,
                                              LaneWidth& width)
{
    int checker = sub_node->QueryDoubleAttribute("sOffset", &width.sOffset);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane width sOffset attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

	  checker = sub_node->QueryDoubleAttribute("a", &width.a);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane width a attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

	  checker = sub_node->QueryDoubleAttribute("b", &width.b);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane width b attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

	  checker = sub_node->QueryDoubleAttribute("c", &width.c);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane width c attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

	  checker = sub_node->QueryDoubleAttribute("d", &width.d);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane width d attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
     return Status::OK();
}

Status LanesOdrParser::ParseRoadMark(const tinyxml2::XMLElement& xml_node,
                                  Lane* lane) {
  auto sub_node = xml_node.FirstChildElement("roadMark");
  while (sub_node) {
    LaneRoadMark roadMark;
    int checker = sub_node->QueryDoubleAttribute("sOffset", &roadMark.sOffset);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane roadMark sOffset attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

	  std::string type;
	  checker = UtilXmlParser::QueryStringAttribute(*sub_node, "type", &type);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse roadMark type.";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    RoadMarkType roadMark_type;
    RETURN_IF_ERROR(ToRoadMarkType(type, &roadMark_type));
    roadMark.type = roadMark_type;

    std::string color;
	  checker = UtilXmlParser::QueryStringAttribute(*sub_node, "color", &color);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse roadMark color.";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    RETURN_IF_ERROR(ToRoadMarkColor(color, roadMark.color));

    checker = sub_node->QueryDoubleAttribute("width", &roadMark.width);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane roadMark width attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }

    std::string laneChange;
	  checker = UtilXmlParser::QueryStringAttribute(*sub_node, "laneChange", &laneChange);
    if (checker != tinyxml2::XML_SUCCESS) {
      // some xodr file may have no laneChange attribute
      laneChange = "both";
      //std::string err_msg = "Error parse roadMark laneChange.";
      //return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    roadMark.laneChange = laneChange;

    lane->roadMarks.push_back(roadMark);
    sub_node = sub_node->NextSiblingElement("roadMark");
  }


  return Status::OK();
}

Status LanesOdrParser::ParseSpeed(const tinyxml2::XMLElement& xml_node,
                                  Lane* lane) {
  double max_speed = 0.0;
  std::string speedStr;
  auto sub_node = xml_node.FirstChildElement("speed");
  if (sub_node) {
    int checker = UtilXmlParser::QueryStringAttribute(*sub_node, "max", &speedStr);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane speed max attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    if (speedStr == "no limit" || speedStr == "undefined") {
      max_speed = 0.0;
    } else {
      max_speed = atof(speedStr.c_str());
      if (max_speed == 0.0) {
        printf("Warning: wrong max speed string for the lane");
      }
    }
    lane->speedMax = max_speed;

    std::string unit;
	  checker = UtilXmlParser::QueryStringAttribute(*sub_node, "unit", &unit);
    if (checker != tinyxml2::XML_SUCCESS) {
      unit = "m/s";
      //std::string err_msg = "Error parse lane speed unit.";
      //return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    lane->speedUnit = apollo::hdmap::adapter::UtilOdrParser::ToSpeedUnit(unit);

	  double sOffset;
	  checker = sub_node->QueryDoubleAttribute("sOffset", &sOffset);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane speed sOffset attribute";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    lane->speedOffset = sOffset;
  }
  else
  {
    // Protect code 
    lane->speedMax = FLAGS_lane_max_speed;
    lane->speedUnit = KM_PER_H;
    lane->speedOffset = 0.0;
  }
  

  return Status::OK();
}


Status LanesOdrParser::ToLaneType(const std::string& type,
                                    LaneType* lane_type) {
  CHECK_NOTNULL(lane_type);

  std::string upper_str = UtilXmlParser::ToUpper(type);

// types defined in opendrive 1.4 specification
/*
none
driving
stop
shoulder
biking
sidewalk
border
restricted
parking
bidirectional (full name: continuous two-way left turn lane)
median
special1
special2
special3
roadWorks
tram
rail
entry
exit
offRamp
onRamp
*/

  if (upper_str == "NONE") {
    *lane_type = NONE;
  } else if (upper_str == "DRIVING" || upper_str == "RESTRICTED") {
    // what is restricted???
    *lane_type = DRIVING;
  } else if (upper_str == "SIDEWALK") {
    *lane_type = SIDEwALK;
  } else {
    *lane_type = NONE;
  }

  return Status::OK();
}

Status LanesOdrParser::ParseLaneLink(const tinyxml2::XMLElement& xml_node,
                                   Lane* lane) {
  CHECK_NOTNULL(lane);
  int checker;
  std::string id;

  Link& link = lane->link;
  link.predecessor = NULL;
  link.successor = NULL;
  const tinyxml2::XMLElement* sub_node =
      xml_node.FirstChildElement("predecessor");
  if (sub_node) {
	  checker = UtilXmlParser::QueryStringAttribute(*sub_node, "id", &id);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane link predecessor.";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    link.predecessor = new Predecessor();
    link.predecessor->elementId = id;
  }

  id = "";
  sub_node = xml_node.FirstChildElement("successor");
  if (sub_node) {
    checker = UtilXmlParser::QueryStringAttribute(*sub_node, "id", &id);
    if (checker != tinyxml2::XML_SUCCESS) {
      std::string err_msg = "Error parse lane link successor.";
      return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
    }
    link.successor = new Predecessor();
    link.successor->elementId = id;
  }

  sub_node = xml_node.FirstChildElement("neighbor");
  if (sub_node) {
    printf("Unsupport neighbor node in lane link.\n");
  }

  return Status::OK();
}

Status LanesOdrParser::ToRoadMarkType(const std::string& type,
                                 RoadMarkType* roadmark_type) {
  CHECK_NOTNULL(roadmark_type);

  std::string upper_type = UtilXmlParser::ToUpper(type);
  *roadmark_type = ROADMARK_NONE;

  if (upper_type == "BROKEN") {
    *roadmark_type = ROADMARK_BROKEN;
  }
  else if (upper_type == "SOLID") {
    *roadmark_type = ROADMARK_SOLID;
  }
  else if (upper_type == "NONE") {
    *roadmark_type = ROADMARK_NONE;
  }
  else if (upper_type == "SOLID SOLID") {
    *roadmark_type = ROADMARK_SOLID_SOLID;
  }
  else if (upper_type == "SOLID BROKEN") {
    *roadmark_type = ROADMARK_SOLID_BROKEN;
  }
  else if (upper_type == "BROKEN SOLID") {
    *roadmark_type = ROADMARK_BROKEN_SOLID;
  }
  else if (upper_type == "BROKEN BROKEN") {
    *roadmark_type = ROADMARK_BROKEN_BROKEN;
  }
  else if (upper_type == "BOTTS DOTS") {
    *roadmark_type = ROADMARK_BOTTS_DOTS;
  }
  else if (upper_type == "GRASS") {
    *roadmark_type = ROADMARK_GRASS;
  }
  else if (upper_type == "CURB") {
    *roadmark_type = ROADMARK_CURB;
  }

  return Status::OK();
}

Status LanesOdrParser::ToRoadMarkColor(const std::string& color,
                                 RoadMarkColor& roadmark_color) {
  std::string upper_color = UtilXmlParser::ToUpper(color);
  roadmark_color = ROADMARK_STANDARD;

  if (upper_color == "STANDARD") {
    roadmark_color = ROADMARK_STANDARD;
  }
  else if (upper_color == "BLUE") {
    roadmark_color = ROADMARK_BLUE;
  }
  else if (upper_color == "GREEN") {
    roadmark_color = ROADMARK_GREEN;
  }
  else if (upper_color == "RED") {
    roadmark_color = ROADMARK_RED;
  }
  else if (upper_color == "WHITE") {
    roadmark_color = ROADMARK_WHITE;
  }
  else if (upper_color == "YELLOW") {
    roadmark_color = ROADMARK_YELLOW;
  }

  return Status::OK();
}

}  // namespace adapter
}  // namespace hdmap
}  // namespace apollo
